template <class T>
inline Matrix44<T>::Matrix44(const T* arr)
{
	assert(arr);
	memcpy_s(m, MATRIX44_SIZE, arr, MATRIX44_SIZE);
}

template <class T>
Matrix44<T>& Matrix44<T>::operator=(const Matrix44<T>& other)
{
	for (int i=0;i<16;++i)
	{
		m[i] = other[i];
	}
	return (*this);
}

template <class T>
Matrix44<T> Matrix44<T>::operator*(const Matrix44<T>& other) const
{
	Matrix44<T> result;

	result.m[0]  = (other.m[0]*m[0])+(other.m[4]*m[1])+(other.m[8]*m[2])+(other.m[12]*m[3]);
	result.m[1]  = (other.m[1]*m[0])+(other.m[5]*m[1])+(other.m[9]*m[2])+(other.m[13]*m[3]);
	result.m[2]  = (other.m[2]*m[0])+(other.m[6]*m[1])+(other.m[10]*m[2])+(other.m[14]*m[3]);
	result.m[3]  = (other.m[3]*m[0])+(other.m[7]*m[1])+(other.m[11]*m[2])+(other.m[15]*m[3]);

	result.m[4]  = (other.m[0]*m[4])+(other.m[4]*m[5])+(other.m[8]*m[6])+(other.m[12]*m[7]);
	result.m[5]  = (other.m[1]*m[4])+(other.m[5]*m[5])+(other.m[9]*m[6])+(other.m[13]*m[7]);
	result.m[6]  = (other.m[2]*m[4])+(other.m[6]*m[5])+(other.m[10]*m[6])+(other.m[14]*m[7]);
	result.m[7]  = (other.m[3]*m[4])+(other.m[7]*m[5])+(other.m[11]*m[6])+(other.m[15]*m[7]);

	result.m[8]  = (other.m[0]*m[8])+(other.m[4]*m[9])+(other.m[8]*m[10])+(other.m[12]*m[11]);
	result.m[9]  = (other.m[1]*m[8])+(other.m[5]*m[9])+(other.m[9]*m[10])+(other.m[13]*m[11]);
	result.m[10] = (other.m[2]*m[8])+(other.m[6]*m[9])+(other.m[10]*m[10])+(other.m[14]*m[11]);
	result.m[11] = (other.m[3]*m[8])+(other.m[7]*m[9])+(other.m[11]*m[10])+(other.m[15]*m[11]);

	result.m[12] = (other.m[0]*m[12])+(other.m[4]*m[13])+(other.m[8]*m[14])+(other.m[12]*m[15]);
	result.m[13] = (other.m[1]*m[12])+(other.m[5]*m[13])+(other.m[9]*m[14])+(other.m[13]*m[15]);
	result.m[14] = (other.m[2]*m[12])+(other.m[6]*m[13])+(other.m[10]*m[14])+(other.m[14]*m[15]);
	result.m[15] = (other.m[3]*m[12])+(other.m[7]*m[13])+(other.m[11]*m[14])+(other.m[15]*m[15]);

	return result;
}

template <class T>
Vector4<T> Matrix44<T>::operator*(const Vector4<T>& vector) const
{
	// column major multiply
	Vector4<T> result;
	result.x = m[0]*vector.x+m[1]*vector.y+m[2]*vector.z+m[3]*vector.w;
	result.y = m[4]*vector.x+m[5]*vector.y+m[6]*vector.z+m[7]*vector.w;
	result.z = m[8]*vector.x+m[9]*vector.y+m[10]*vector.z+m[11]*vector.w;
	result.w = m[12]*vector.x+m[13]*vector.y+m[14]*vector.z+m[15]*vector.w;
	return result;
}

template <class T>
Matrix44<T>& Matrix44<T>::operator*=(const Matrix44<T>& matrix)
{
	(*this) = (*this)*matrix;
	return (*this);
}

//template <class T>
//Matrix44<T> Matrix44<T>::affineInverse() const
//{
//	// (AB)-1 = B-1 * A-1
//	Matrix44<double> r,t;
//	RotationPart(r);
//	TranslationPart(t);
//	t.TranslationInverse();
//	r.Transpose();
//	Matrix44<T>& result = t*r;
//	//memcpy(m,result.m,sizeof(T)*16);
//	return result;
//}


template <class T>
Matrix44<T> Matrix44<T>::inverse() const
{
	T a0 = m[ 0]*m[ 5] - m[ 1]*m[ 4];
	T a1 = m[ 0]*m[ 6] - m[ 2]*m[ 4];
	T a2 = m[ 0]*m[ 7] - m[ 3]*m[ 4];
	T a3 = m[ 1]*m[ 6] - m[ 2]*m[ 5];
	T a4 = m[ 1]*m[ 7] - m[ 3]*m[ 5];
	T a5 = m[ 2]*m[ 7] - m[ 3]*m[ 6];
	T b0 = m[ 8]*m[13] - m[ 9]*m[12];
	T b1 = m[ 8]*m[14] - m[10]*m[12];
	T b2 = m[ 8]*m[15] - m[11]*m[12];
	T b3 = m[ 9]*m[14] - m[10]*m[13];
	T b4 = m[ 9]*m[15] - m[11]*m[13];
	T b5 = m[10]*m[15] - m[11]*m[14];

	T det = a0*b5 - a1*b4 + a2*b3 + a3*b2 - a4*b1 + a5*b0;
	if (abs(det) > 0)
	{
		Matrix44<T> inverse;
		inverse.m[ 0] = + m[ 5]*b5 - m[ 6]*b4 + m[ 7]*b3;
		inverse.m[ 4] = - m[ 4]*b5 + m[ 6]*b2 - m[ 7]*b1;
		inverse.m[ 8] = + m[ 4]*b4 - m[ 5]*b2 + m[ 7]*b0;
		inverse.m[12] = - m[ 4]*b3 + m[ 5]*b1 - m[ 6]*b0;
		inverse.m[ 1] = - m[ 1]*b5 + m[ 2]*b4 - m[ 3]*b3;
		inverse.m[ 5] = + m[ 0]*b5 - m[ 2]*b2 + m[ 3]*b1;
		inverse.m[ 9] = - m[ 0]*b4 + m[ 1]*b2 - m[ 3]*b0;
		inverse.m[13] = + m[ 0]*b3 - m[ 1]*b1 + m[ 2]*b0;
		inverse.m[ 2] = + m[13]*a5 - m[14]*a4 + m[15]*a3;
		inverse.m[ 6] = - m[12]*a5 + m[14]*a2 - m[15]*a1;
		inverse.m[10] = + m[12]*a4 - m[13]*a2 + m[15]*a0;
		inverse.m[14] = - m[12]*a3 + m[13]*a1 - m[14]*a0;
		inverse.m[ 3] = - m[ 9]*a5 + m[10]*a4 - m[11]*a3;
		inverse.m[ 7] = + m[ 8]*a5 - m[10]*a2 + m[11]*a1;
		inverse.m[11] = - m[ 8]*a4 + m[ 9]*a2 - m[11]*a0;
		inverse.m[15] = + m[ 8]*a3 - m[ 9]*a1 + m[10]*a0;

		T invDet = ((T)1)/det;
		inverse.m[ 0] *= invDet;
		inverse.m[ 1] *= invDet;
		inverse.m[ 2] *= invDet;
		inverse.m[ 3] *= invDet;
		inverse.m[ 4] *= invDet;
		inverse.m[ 5] *= invDet;
		inverse.m[ 6] *= invDet;
		inverse.m[ 7] *= invDet;
		inverse.m[ 8] *= invDet;
		inverse.m[ 9] *= invDet;
		inverse.m[10] *= invDet;
		inverse.m[11] *= invDet;
		inverse.m[12] *= invDet;
		inverse.m[13] *= invDet;
		inverse.m[14] *= invDet;
		inverse.m[15] *= invDet;

		return inverse;
	}

	return IdentityMatrix;
}

template <class T>
Matrix44<T>& Matrix44<T>::rotation(const Vector3<T>& axis, T radius)
{
	T s = sin(radius);
	T c = cos(radius);

	vec3f normAxis = axis;
	normAxis.normalize();

	T ux = normAxis.x;
	T uy = normAxis.y;
	T uz = normAxis.z;

	m[0]  = c + (1-c) * ux;
	m[1]  = (1-c) * ux*uy + s*uz;
	m[2]  = (1-c) * ux*uz - s*uy;
	m[3]  = 0;

	m[4]  = (1-c) * uy*ux - s*uz;
	m[5]  = c + (1-c) * pow(uy,2);
	m[6]  = (1-c) * uy*uz + s*ux;
	m[7]  = 0;

	m[8]  = (1-c) * uz*ux + s*uy;
	m[9]  = (1-c) * uz*uz - s*ux;
	m[10] = c + (1-c) * pow(uz,2);
	m[11] = 0;

	m[12] = 0;
	m[13] = 0;
	m[14] = 0;
	m[15] = 1;

	return (*this);
}

template <class T>
Matrix44<T>& Matrix44<T>::rotation(const Vector3<T>& i,const Vector3<T>& j,const Vector3<T>& k)
{
	m[0] = i.x;m[1]=i.y;m[2]=i.z;m[3]=0;
	m[4] = j.x;m[5]=j.y;m[6]=j.z;m[7]=0;
	m[8] = k.x;m[9]=k.y;m[10]=k.z;m[11]=0;
	m[12] = 0;m[13]=0;m[14]=0;m[15]=1;
	return (*this);
}

template <class T>
Matrix44<T>& Matrix44<T>::translation(const Vector3<T>& xlate)
{
	m[12] = xlate.x;
	m[13] = xlate.y;
	m[14] = xlate.z;

	return(*this);
}

template <class T>
Matrix44<T> Matrix44<T>::transpose() const
{
	Matrix44<T> result;
	result.m[0]=m[0];result.m[1]=m[4];result.m[2]=m[8];result.m[3]=m[12];
	result.m[4]=m[1];result.m[5]=m[5];result.m[6]=m[9];result.m[7]=m[13];
	result.m[8]=m[2];result.m[9]=m[6];result.m[10]=m[10];result.m[11]=m[14];
	result.m[12]=m[3];result.m[13]=m[7];result.m[14]=m[11];result.m[15]=m[15];

	return result;
}

template <class T>
inline void Matrix44<T>::identity()
{
	m[0]=1; m[4]=0; m[8] =0; m[12]=0;
	m[1]=0; m[5]=1; m[9] =0; m[13]=0;
	m[2]=0; m[6]=0; m[10]=1; m[14]=0;
	m[3]=0; m[7]=0; m[11]=0; m[15]=1;
}

template <class T>
inline T& Matrix44<T>::operator()(unsigned int i, unsigned int j)
{
	return m[4*i + j];
}

template <class T>
inline T Matrix44<T>::operator()(unsigned int i, unsigned int j) const
{
	return m[4*i + j];
}